#include "os_config.h"
#include "os_type.h"
#include "os_routine.h"
#include "os_cm3.h"
#include "os_task.h"
#include "string.h"
#include "stdio.h"
#include "Q_Shell.h"
#if OS_CONFIG_STK_CHECK_EN
#define OS_TASK_STACK_FILL_BYTE 0xa5 
#endif
OS_TCB OSTaskAllTask[OS_CONFIG_TASK_MAX_NUM];
static volatile u32 priOSTaskSysTicks=0;
static u32 priOSTaskSchedLock = 0;
volatile u32 OSTaskReadyList[2];
QSH_VAR_REG(OSTaskReadyList,"volatile u32 OSTaskReadyList[2]","u32");
volatile u32 OSTaskDelayList[2];
QSH_VAR_REG(OSTaskDelayList,"volatile u32 OSTaskDelayList[2]","u32");
volatile u32 OSTaskSuspendList[2];
QSH_VAR_REG(OSTaskSuspendList,"volatile u32 OSTaskSuspendList[2]","u32");
volatile u32 OSTaskBlockList[2];
QSH_VAR_REG(OSTaskBlockList,"volatile u32 OSTaskBlockList[2]","u32");
volatile u32 OSTaskRunList[2];
QSH_VAR_REG(OSTaskRunList,"volatile u32 OSTaskRunList[2]","u32");
volatile u32 OSTaskDelaySuspendList[2];
QSH_VAR_REG(OSTaskDelaySuspendList,"volatile u32 OSTaskDelaySuspendList[2]","u32");
volatile u32 OSTaskBlockSuspendList[2];
QSH_VAR_REG(OSTaskBlockSuspendList,"volatile u32 OSTaskBlockSuspendList[2]","u32");
u32 *OSTaskReadyListBitBand;
u32 *OSTaskDelayListBitBand;
u32 *OSTaskSuspendListBitBand;
u32 *OSTaskBlockListBitBand;
u32 *OSTaskRunListBitBand;						
u32 *OSTaskDelaySuspendListBitBand;
u32 *OSTaskBlockSuspendListBitBand;

u32 OSTaskGetPrio(volatile u32 *TaskList)
{
	if(TaskList[1])
	{
		return (63-OSM3CntLeadZeros(TaskList[1]));
	}
	else if(TaskList[0])
	{	
		return (31-OSM3CntLeadZeros(TaskList[0]));
	}
	else
		return NO_TASK_IN_THIS_STATE;	
}

static void priOSTaskStateMachineInit(void)
{
	u32 Addr;
	OSTaskReadyList[0]=0x00000000;
	OSTaskReadyList[1]=0x00000000;
	OSTaskDelayList[0]=0x00000000;
	OSTaskDelayList[1]=0x00000000;
	OSTaskSuspendList[0]=0x00000000;
	OSTaskSuspendList[1]=0x00000000;
	OSTaskBlockList[0]=0x00000000;
	OSTaskBlockList[1]=0x00000000;
	OSTaskRunList[0]=0x00000000;
	OSTaskRunList[1]=0x00000000;
	OSTaskDelaySuspendList[0]=0x00000000;
	OSTaskDelaySuspendList[1]=0x00000000;
	OSTaskBlockSuspendList[0]=0x00000000;
	OSTaskBlockSuspendList[1]=0x00000000;
	Addr=(u32)OSTaskReadyList;
	OSTaskReadyListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
	Addr=(u32)OSTaskDelayList;
	OSTaskDelayListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
	Addr=(u32)OSTaskSuspendList;
	OSTaskSuspendListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
	Addr=(u32)OSTaskBlockList;
	OSTaskBlockListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
	Addr=(u32)OSTaskRunList;
	OSTaskRunListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
	Addr=(u32)OSTaskDelaySuspendList;
	OSTaskDelaySuspendListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
	Addr=(u32)OSTaskBlockSuspendList;
	OSTaskBlockSuspendListBitBand=(u32 *)(0x22000000 + (Addr-0x20000000)*32);
}

static void OSTaskSch(void)
{
	u32 HighestPrioInReady;
	u32 PrioInRun;
	OSEnterCritical();
	if(priOSTaskSchedLock == 0)
	{
		HighestPrioInReady=OSTaskGetPrio(OSTaskReadyList);
		PrioInRun=OSTaskGetPrio(OSTaskRunList);
		if(HighestPrioInReady == NO_TASK_IN_THIS_STATE )
		{
			OSExitCritical();
			return;
		}
		if(PrioInRun == NO_TASK_IN_THIS_STATE )
		{
			OSTaskStateMachineChangeState(HighestPrioInReady,TSCR_READY_TO_RUN);
			OSRoutTo(&(OSTaskAllTask[HighestPrioInReady].RoutineHandle));
		}
		else if( PrioInRun < HighestPrioInReady )
		{
			OSTaskStateMachineChangeState(PrioInRun,TSCR_RUN_TO_READY);
			OSTaskStateMachineChangeState(HighestPrioInReady,TSCR_READY_TO_RUN);
			OSRoutTo(&(OSTaskAllTask[HighestPrioInReady].RoutineHandle));
		}
	}
	OSExitCritical();
}

void OSTaskStateMachineChangeState(u32 TaskPrio,OS_TASK_STATE_CHANGE_ROUTE Route)
{
	OSEnterCritical();
	switch (Route)
	{
		case TSCR_SLEEP_TO_READY:
			SET_READY_LIST(TaskPrio);
			break;
		case TSCR_READY_TO_RUN:
			CLR_READY_LIST(TaskPrio);
			SET_RUN_LIST(TaskPrio);
			break;
		case TSCR_READY_TO_SUSPEND:
			CLR_READY_LIST(TaskPrio);
			SET_SUSPEND_LIST(TaskPrio);
			break;
		case TSCR_RUN_TO_BLOCK:
			CLR_RUN_LIST(TaskPrio);
			SET_BLOCK_LIST(TaskPrio);
			OSTaskSch();
			break;
		case TSCR_RUN_TO_READY:
			CLR_RUN_LIST(TaskPrio);
			SET_READY_LIST(TaskPrio);
			break;
		case TSCR_RUN_TO_SUSPEND:
			CLR_RUN_LIST(TaskPrio);
			SET_SUSPEND_LIST(TaskPrio);
			OSTaskSch();
			break;
		case TSCR_RUN_TO_DELAY:
			CLR_RUN_LIST(TaskPrio);
			SET_DELAY_LIST(TaskPrio);
			OSTaskSch();
			break;
		case TSCR_DELAY_TO_READY:
			CLR_DELAY_LIST(TaskPrio);
			SET_READY_LIST(TaskPrio);
			OSTaskSch();
			break;
		case TSCR_DELAY_TO_DELAYSUSPEND:
			CLR_DELAY_LIST(TaskPrio);
			SET_DELAYSUSPEND_LIST(TaskPrio);
			break;
		case TSCR_SUSPEND_TO_READY:
			CLR_SUSPEND_LIST(TaskPrio);
			SET_READY_LIST(TaskPrio);
			OSTaskSch();
			break;
		case TSCR_BLOCK_TO_BLOCKSUSPEND:
			CLR_BLOCK_LIST(TaskPrio);
			SET_BLOCKSUSPEND_LIST(TaskPrio);
			break;
		case TSCR_BLOCK_TO_READY:
			CLR_BLOCK_LIST(TaskPrio);
			SET_READY_LIST(TaskPrio);
			OSTaskSch();
			break;
		case TSCR_BLOCKSUSPEND_TO_BLOCK:
			CLR_BLOCKSUSPEND_LIST(TaskPrio);
			SET_BLOCK_LIST(TaskPrio);
			break;
		case TSCR_BLOCKSUSPEND_TO_SUSPEND:
			CLR_BLOCKSUSPEND_LIST(TaskPrio);
			SET_SUSPEND_LIST(TaskPrio);
			break;
		case TSCR_DELAYSUSPEND_TO_DELAY:
			CLR_DELAYSUSPEND_LIST(TaskPrio);
			SET_DELAY_LIST(TaskPrio);
			break;
		case TSCR_DELAYSUSPEND_TO_SUSPEND:
			CLR_DELAYSUSPEND_LIST(TaskPrio);
			SET_SUSPEND_LIST(TaskPrio);
			break;
		default:
			break;
	}
	OSExitCritical();
}

OS_STATE OSCreatTask(void (*Code)(void *), void *Para, void *StkBuf,u32 StkSize, const u8 *TaskName,u32 Prio)
{
	if(Prio>=OS_CONFIG_TASK_MAX_NUM || StkSize<64 )
		return OS_FALSE;
	OSEnterCritical();
	#if OS_CONFIG_STK_CHECK_EN
	memset( StkBuf, OS_TASK_STACK_FILL_BYTE, StkSize );
	OSTaskAllTask[Prio].StkBuf  = StkBuf;
	OSTaskAllTask[Prio].StkSize = StkSize;
	#endif
	OSTaskAllTask[Prio].RoutineHandle = OSRoutCreat(Code,Para,(u32)StkBuf+StkSize);
	OSTaskAllTask[Prio].Name = TaskName;
	OSTaskAllTask[Prio].DelayTicks = 0;
	OSTaskAllTask[Prio].BlockTicks = 0;
	OSTaskAllTask[Prio].WaitEvent =  0;
	OSTaskAllTask[Prio].IfOverTime = OS_FALSE;
	OSTaskStateMachineChangeState(Prio,TSCR_SLEEP_TO_READY);
	OSExitCritical();
	return OS_TRUE;
}

void OSInit(void)
{
	memset((void *)OSTaskAllTask,0,sizeof(OSTaskAllTask));
	priOSTaskStateMachineInit();
	OSM3Init();
}

void OSStart(void)
{
	u32 HighestPrioInReady;	
	HighestPrioInReady=OSTaskGetPrio(OSTaskReadyList);
	CLR_READY_LIST(HighestPrioInReady);
	SET_RUN_LIST(HighestPrioInReady);
	OSRoutStart(&(OSTaskAllTask[HighestPrioInReady].RoutineHandle));
}

void OSDelayTask(u32 Ticks)
{
	u32 PrioInRun;
	OSEnterCritical();
	PrioInRun = OSTaskGetPrio(OSTaskRunList);
	OSTaskAllTask[PrioInRun].DelayTicks = Ticks;
	OSTaskStateMachineChangeState(PrioInRun,TSCR_RUN_TO_DELAY);
	OSExitCritical();
}

void OSTaskTick(void)
{
	u32 Prio;
	priOSTaskSysTicks++;
	for(Prio=0;Prio<OS_CONFIG_TASK_MAX_NUM;Prio++)
	{
		if(OSTaskAllTask[Prio].RoutineHandle==0)
			break;
		if(GET_DELAY_LIST(Prio))
		{
			if( OSTaskAllTask[Prio].DelayTicks>0 )
			{
				(OSTaskAllTask[Prio].DelayTicks)--;
				if(OSTaskAllTask[Prio].DelayTicks==0)
					OSTaskStateMachineChangeState(Prio,TSCR_DELAY_TO_READY);
			}		
		}
		else if(GET_DELAYSUSPEND_LIST(Prio))
		{
			if( OSTaskAllTask[Prio].DelayTicks>0 )
			{
				(OSTaskAllTask[Prio].DelayTicks)--;
				if(OSTaskAllTask[Prio].DelayTicks==0)
					OSTaskStateMachineChangeState(Prio,TSCR_DELAYSUSPEND_TO_SUSPEND);
			}			
		}
		else if(GET_BLOCK_LIST(Prio))
		{
			if( OSTaskAllTask[Prio].BlockTicks>0 )
			{
				(OSTaskAllTask[Prio].BlockTicks)--;
				if(OSTaskAllTask[Prio].BlockTicks==0)
				{
					(OSTaskAllTask[Prio].WaitEvent)->WaitListBitBand[Prio]=0;
					OSTaskAllTask[Prio].IfOverTime = OS_TRUE;
					OSTaskStateMachineChangeState(Prio,TSCR_BLOCK_TO_READY);
				}
			}		
		}
		else if(GET_BLOCKSUSPEND_LIST(Prio))
		{
			if( OSTaskAllTask[Prio].BlockTicks>0 )
			{
				(OSTaskAllTask[Prio].BlockTicks)--;
				if(OSTaskAllTask[Prio].BlockTicks==0)
				{
					(OSTaskAllTask[Prio].WaitEvent)->WaitListBitBand[Prio]=0;
					OSTaskAllTask[Prio].IfOverTime = OS_TRUE;
					OSTaskStateMachineChangeState(Prio,TSCR_BLOCKSUSPEND_TO_SUSPEND);
				}
			}		
		}	
	}	
}

void OSSuspendTask(u32 Prio)
{
	if(OSTaskAllTask[Prio].RoutineHandle==0)
		return ;
	OSEnterCritical();
	if(GET_SUSPEND_LIST(Prio) || GET_DELAYSUSPEND_LIST(Prio) || GET_BLOCKSUSPEND_LIST(Prio) )
	{
		OSExitCritical();
		return ;
	}
	if(GET_READY_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_READY_TO_SUSPEND);
	else if(GET_RUN_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_RUN_TO_SUSPEND);
	else if(GET_DELAY_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_DELAY_TO_DELAYSUSPEND);
	else if(GET_BLOCK_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_BLOCK_TO_BLOCKSUSPEND);
	OSExitCritical();
}
QSH_FUN_REG(OSSuspendTask,"void OSSuspendTask(u32 Prio)");

void OSResumeTask(u32 Prio)
{
	if(OSTaskAllTask[Prio].RoutineHandle==0)
		return;
	OSEnterCritical();
	if( GET_SUSPEND_LIST(Prio)==0 && GET_DELAYSUSPEND_LIST(Prio)==0 && GET_BLOCKSUSPEND_LIST(Prio)==0 )
	{
		OSExitCritical();
		return;
	}
	if(GET_SUSPEND_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_SUSPEND_TO_READY);
	else if(GET_DELAYSUSPEND_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_DELAYSUSPEND_TO_DELAY);
	else if(GET_BLOCKSUSPEND_LIST(Prio))
		OSTaskStateMachineChangeState(Prio,TSCR_BLOCKSUSPEND_TO_BLOCK);
	OSExitCritical();
}
QSH_FUN_REG(OSResumeTask,"void OSResumeTask(u32 Prio)");

void OSSchedLock(void)
{
	priOSTaskSchedLock++;
}
QSH_FUN_REG(OSSchedLock,"void OSSchedLock(void)");

void OSSchedUnlock(void)
{
	OSEnterCritical();
	if(priOSTaskSchedLock>0)
		priOSTaskSchedLock--;
	else
		OSTaskSch();
	OSExitCritical();
}
QSH_FUN_REG(OSSchedUnlock,"void OSSchedUnlock(void)");

u32 OSGetCurrentTick(void)
{
	return priOSTaskSysTicks;
}
QSH_FUN_REG(OSGetCurrentTick,"u32 OSGetCurrentTick(void)");

u32 OSGetCurrentSysMs(void)
{
	return priOSTaskSysTicks*1000/OS_CONFIG_TICK_RATE_HZ;
}
QSH_FUN_REG(OSGetCurrentSysMs,"u32 OSGetCurrentSysMs(void)");

#if OS_CONFIG_STK_CHECK_EN
void OSStkCheck(char *WriteBuf)
{
	u32 i,j;
	char TempString[32];
	*WriteBuf = 0;
	sprintf(TempString,"TaskName StkTotal StkRemain\r\n");
	strcat(WriteBuf,TempString);
	for(i=0;i<OS_CONFIG_TASK_MAX_NUM;i++)
	{
		if(OSTaskAllTask[i].RoutineHandle==0)
			break;
		for(j=0;j<OSTaskAllTask[i].StkSize;j++)
		{
			if(((u8 *)(OSTaskAllTask[i].StkBuf))[j]!=OS_TASK_STACK_FILL_BYTE)
				break;
		}
		sprintf(TempString,"%s %d  %d\n\r",OSTaskAllTask[i].Name,OSTaskAllTask[i].StkSize,j);	
		strcat(WriteBuf,TempString);
	}
}

OS_STATE OSStkChk(u32 RemainSize)
{
	u32 i,j;
	for(i=0;i<OS_CONFIG_TASK_MAX_NUM;i++)
	{
		if(OSTaskAllTask[i].RoutineHandle==0)
			break;
		for(j=0;j<OSTaskAllTask[i].StkSize;j++)
		{
			if(((u8 *)(OSTaskAllTask[i].StkBuf))[j]!=OS_TASK_STACK_FILL_BYTE)
				break;
		}
		if(j<RemainSize)
			return OS_FALSE;
	}
	return OS_TRUE;
}
#endif
